#include <stdint.h>
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <stdbool.h>
#include <mxu2.h>
#include <jzmxu128.h>
#ifdef DMALLOC
#include <imp/dmalloc.h>
#endif
void filter2D(uint8_t* _dst, uint8_t* psn_addr, uint8_t* _srcOld, uint8_t* _srcNew, int width, int height, int block_h_size, int block_w_size, int skip_h_size, int skip_w_size,
	      int blur_radius_size, int blur_diff_thres, int pixel_blur_diff_thres, int diff_num_thres, int ori_width, int ori_height, int psn_max_num, int mv_block_limit,
	      int block_h_size_ds, int block_w_size_ds, int skip_h_size_ds, int skip_w_size_ds, int blur_radius_size_ds, int blur_diff_thres_change_ds, int mv_block_limit_ds)
{
	int i,j,m,n,k,l;
	int block_h_size_new, block_w_size_new;
	int skip_h_size_new, skip_w_size_new;
	int blur_radius_size_new, blur_diff_thres_new, pixel_blur_diff_thres_new, mv_block_limit_new, diff_num_thres_new;
	int block_h_num;
	int block_w_num;
	int block_h_res;
	int block_w_res;
	int blur_window_size;
	int data_diff_cmp_thres, data_diff_cmp_thres_2;
	int h_skip_block_num, w_skip_block_num;
	int mv_block_num;
	int new_block_width, new_block_height;
	int count, block_index;
	int data_sum1, data_sum2;
	int cur_block_h, cur_block_w;
	int blur_diff_array[20];
	int data_diff;
	uint8_t data_result, data_result_old, data_result_tmp;
	int h_last_tmp, w_last_tmp;

	if(width == ori_width){
		block_h_size_new          = block_h_size    ;
		block_w_size_new          = block_w_size    ;
		skip_h_size_new           = skip_h_size     ;
		skip_w_size_new           = skip_w_size     ;
		blur_radius_size_new      = blur_radius_size;
		blur_diff_thres_new       = blur_diff_thres ;
		pixel_blur_diff_thres_new = pixel_blur_diff_thres ;
		diff_num_thres_new        = diff_num_thres  ;
		mv_block_limit_new        = mv_block_limit  ;
	}
	else{
		block_h_size_new          = block_h_size_ds  ;
		block_w_size_new          = block_w_size_ds  ;
		skip_h_size_new           = skip_h_size_ds   ;
		skip_w_size_new           = skip_w_size_ds   ;
		blur_radius_size_new      = blur_radius_size_ds;
		blur_diff_thres_new       = blur_diff_thres - blur_diff_thres_change_ds ;
		pixel_blur_diff_thres_new = pixel_blur_diff_thres - blur_diff_thres_change_ds;
		diff_num_thres_new        = diff_num_thres  ;
		mv_block_limit_new        = mv_block_limit_ds;
	}

	block_h_num = (height - 8) / block_h_size_new;
	block_w_num = (width - 8) / block_w_size_new;
	block_h_res = (height - 8) % block_h_size_new;
	block_w_res = (width - 8) % block_w_size_new;
	blur_window_size = (2 * blur_radius_size_new) * (2 * blur_radius_size_new);
	data_diff_cmp_thres   = blur_window_size * blur_diff_thres_new;
	data_diff_cmp_thres_2 = blur_window_size * pixel_blur_diff_thres_new;

	h_skip_block_num = (block_h_size_new - 2 * blur_radius_size_new) / skip_h_size_new + 1;
	w_skip_block_num = (block_w_size_new - 2 * blur_radius_size_new) / skip_w_size_new + 1;

	mv_block_num = 0;

	/*前两个循环是为了切换block*/
	for( i = 0; i < block_h_num; i++){
		new_block_height = i * block_h_size_new + 8; // * width; //切换竖直方向的块，i * 32 * 640（一行的pixel大小）, 32*640是在竖直方向上跳过了多少个块
		for( j = 0; j < block_w_num; j++){
			new_block_width = j * block_w_size_new + 8; // 同竖直方向的概念，这里操作水平方向。
			/*这两个循环可以计算一个block内的值*/
			count = 0;
			block_index = 0;
			for(m = blur_radius_size_new; m <= block_h_size_new - blur_radius_size_new; m = m + skip_h_size_new){
				for(n = blur_radius_size_new; n <= block_w_size_new - blur_radius_size_new; n = n + skip_w_size_new){
					data_sum1 = 0;
					data_sum2 = 0;
					for(l = -blur_radius_size_new; l < blur_radius_size_new; l++){
						for(k = -blur_radius_size_new; k < blur_radius_size_new; k++){
							cur_block_h = new_block_height + m + l;
							cur_block_w = new_block_width + n + k;
							data_sum1 = data_sum1 + *(_srcNew + cur_block_h * width + cur_block_w);
							data_sum2 = data_sum2 + *(_srcOld + cur_block_h * width + cur_block_w);
						}
					}

					data_diff = abs(data_sum1 - data_sum2);
					blur_diff_array[block_index] = data_diff;
					block_index = block_index + 1;
					if(data_diff > data_diff_cmp_thres)
						count++;
				}
			}

			if(count >= diff_num_thres_new){
				data_result_tmp = 0;
				mv_block_num = mv_block_num + 1;
			}
			else{
				data_result_tmp = 1;
			}

			block_index = 0;
			for(m = 0; m < h_skip_block_num; m = m + 1){
				for(n = 0; n < w_skip_block_num; n = n + 1){
					if((data_result_tmp == 0) && (mv_block_num <= mv_block_limit_new)){
						data_diff = blur_diff_array[block_index];
						block_index = block_index + 1;
						if(data_diff > data_diff_cmp_thres_2){
							data_result = 0;
						}
						else{
							data_result = 1;
						}
					}
					else
						data_result = data_result_tmp;

					data_result_old = *(psn_addr + i * h_skip_block_num * block_w_num * w_skip_block_num + m * block_w_num * w_skip_block_num + j * w_skip_block_num + n);

					if(data_result != 0){
						if(data_result_old < psn_max_num)
							data_result = data_result_old + 1;
						else
							data_result = psn_max_num;
					}

					*(psn_addr + i * h_skip_block_num * block_w_num * w_skip_block_num + m * block_w_num * w_skip_block_num + j * w_skip_block_num + n) = data_result;

					for( l = 0; l < skip_h_size_new; l++){
						for( k = 0; k < skip_w_size_new / 2; k++){
							*(_dst + (new_block_height + m * skip_h_size_new + l) * width / 2 + new_block_width / 2 +  n * skip_w_size_new / 2 + k) = (data_result << 4) | data_result;
						}
					}
				}
			}
		}
	}

	//padding first 8 lines
	for( i = 0; i < 8; i++){
		for( j = 0; j < width/2; j++){
			*(_dst + i * width / 2 + j) = *(_dst + 8 * width / 2 + j);
		}
	}

	//padding last residual lines
	if(block_h_res != 0){
		h_last_tmp = height - block_h_res;
		for( i = h_last_tmp; i < height; i++){
			for( j = 0; j < width/2; j++){
				*(_dst + i * width / 2 + j) = *(_dst + (h_last_tmp - 1) * width / 2 + j);
			}
		}
	}

	//padding first 8 columns
	for( i = 0; i < height; i++){
		for( j = 0; j < 4; j++){
			*(_dst + i * width / 2 + j) = *(_dst + i * width / 2 + 4);
		}
	}

	//padding last residual columns
	if(block_w_res != 0){
		w_last_tmp = (width - block_w_res) / 2;
		for( i = 0; i < height; i++){
			for( j = w_last_tmp; j < width/2; j++){
				*(_dst + i * width / 2 + j) = *(_dst + i * width / 2 + w_last_tmp - 1);
			}
		}
	}

}

void filter2D_SIMD(uint8_t* _dst, uint8_t* psn_addr, uint8_t* _srcOld, uint8_t* _srcNew, int width, int height, int _block_h_size, int _block_w_size, int _skip_h_size, int _skip_w_size,
		   int _blur_radius_size, int _blur_diff_thres, int _pixel_blur_diff_thres, int _diff_num_thres, int ori_width, int ori_height, int psn_max_num, int _mv_block_limit,
		   int _block_h_size_ds, int _block_w_size_ds, int _skip_h_size_ds, int _skip_w_size_ds, int _blur_radius_size_ds, int _blur_diff_thres_change_ds, int _mv_block_limit_ds)
{
	int i,j,m,n,p,q,k,l;
	int sstep = width;
	int dstep = width>>1;
	int block_h_size;
	int block_w_size;
	int skip_h_size;
	int skip_w_size;
	int blur_radius_size;
	int blur_diff_thres;
	int pixel_blur_diff_thres;
	int diff_num_thres;
	int mv_block_limit;
	int data_result_tmp;
	int mv_block_num = 0;
	if(width == ori_width){
		block_h_size = _block_h_size;
		block_w_size = _block_w_size;
		skip_h_size = _skip_h_size;
		skip_w_size = _skip_w_size;
		blur_radius_size = _blur_radius_size;
		blur_diff_thres = _blur_diff_thres;
		pixel_blur_diff_thres = _pixel_blur_diff_thres;
		diff_num_thres  = _diff_num_thres;
		mv_block_limit  = _mv_block_limit;
	}
	else{
		block_h_size = _block_h_size_ds;
		block_w_size = _block_w_size_ds;
		skip_h_size = _skip_h_size_ds;
		skip_w_size = _skip_w_size_ds;
		blur_radius_size = _blur_radius_size_ds;
		blur_diff_thres = _blur_diff_thres - _blur_diff_thres_change_ds ;
		pixel_blur_diff_thres = _pixel_blur_diff_thres - _blur_diff_thres_change_ds;
		diff_num_thres = _diff_num_thres;
		mv_block_limit = _mv_block_limit_ds;
	}

	int blur_h_size = 2*blur_radius_size;
	int blur_w_size = blur_h_size;
	int cell_h_size = 8;
	int cell_w_size = 8;
	int block_num_inw  = (width-8) / block_w_size;
	int block_num_inh  = (height-8) / block_h_size;
	int blur_num_inwblock = (block_w_size - blur_w_size)/skip_w_size + 1;
	int blur_num_inhblock = (block_h_size - blur_h_size)/skip_h_size + 1;
	int cell_num_inwblur = blur_w_size/cell_w_size;
	int cell_num_inhblur = blur_h_size/cell_h_size;
	int block_w_res = (width-8) % block_w_size;
	int block_h_res = (height-8) % block_h_size;
	int blur_window_size   = blur_w_size*blur_h_size;
	int diff_thres = blur_window_size * blur_diff_thres;
	int diff_thres_2 = blur_window_size * pixel_blur_diff_thres;
	int skip_step = block_num_inw*blur_num_inwblock;

	int new_block_width, new_block_height;
	int new_blur_width, new_blur_height;
	int /*new_cell_width,*/ new_cell_height;
	int new_skip_width, new_skip_height;

	int restore_in_w, restore_in_h;
//	int status, leftnum;
	int woffset = 0;
	int hoffset = 0;
	int16_t sumarray[8];
	bool blur_diff_array[20];
	int block_index = 0;
	int count = 0;
	int data_diff;
	uint8_t data_result, data_result_old;
	unsigned char* old_ptr;
	unsigned char* new_ptr;
//	unsigned char* dst_ptr;

	v16u8 old_line0, old_line1, old_line2, old_line3, old_line4, old_line5, old_line6, old_line7;
	v16u8 new_line0, new_line1, new_line2, new_line3, new_line4, new_line5, new_line6, new_line7;
	v16u8 sum_blur,/* vresult,*/ zero;
	v16u8 cvtbyte_short;
	int cvt[4]={0x03020100, 0x07060504, 0x0b0a0908, 0x0f0e0d0c};
	cvtbyte_short = __mxu2_lu1q(cvt, 0);
	zero = __mxu2_li_b(0);

	/*前两个循环是为了切换block*/
	for( i = 0; i < block_num_inh; i++){
		new_block_height = i * block_h_size + 8; // * width; //切换竖直方向的块，i * 32 * 640（一行的pixel大小）, 32*640是在竖直方向上跳过了多少个块
		for( j = 0; j < block_num_inw; j++){
			new_block_width = j * block_w_size + 8; // 同竖直方向的概念，这里操作水平方向。
			/*这两个循环可以计算一个block内的值*/
			count = 0;
			block_index = 0;
			for(m = 0; m < blur_num_inhblock; m++){
				new_blur_height = m*skip_h_size;
				for(n = 0; n < blur_num_inwblock; n++){
					new_blur_width = n*skip_w_size;

					sum_blur=__mxu2_li_b(0);
					for(p = 0; p < cell_num_inhblur; p++){
						new_cell_height = p*cell_h_size;
						woffset = new_block_width + new_blur_width;
						hoffset = (new_block_height + new_blur_height + new_cell_height)*sstep;
						old_ptr = _srcOld + hoffset;
						new_ptr = _srcNew + hoffset;
						for(q = 0; q < cell_num_inwblur; q++, woffset += cell_w_size){
							/* new_cell_width = q*cell_w_size; */
							/* if(new_block_width + new_blur_width + new_cell_width != woffset){ */
							/* 	printf("error: woffset cal : %d, %d\n",new_block_width + new_blur_width + new_cell_width ,woffset); */
							/* 	return; */
							/* } */
							/*************line[0,3]************/
							old_line0 = __mxu2_lu1q(old_ptr + woffset, 0);
							old_line1 = __mxu2_lu1q(old_ptr + sstep + woffset, 0);
							old_line2 = __mxu2_lu1q(old_ptr + sstep*2 + woffset, 0);
							old_line3 = __mxu2_lu1q(old_ptr + sstep*3 + woffset, 0);

							new_line0 = __mxu2_lu1q(new_ptr + woffset, 0);
							new_line1 = __mxu2_lu1q(new_ptr + sstep + woffset, 0);
							new_line2 = __mxu2_lu1q(new_ptr + sstep*2 + woffset, 0);
							new_line3 = __mxu2_lu1q(new_ptr + sstep*3 + woffset, 0);

							old_line0 = __mxu2_shufv(zero, old_line0, cvtbyte_short);
							old_line1 = __mxu2_shufv(zero, old_line1, cvtbyte_short);
							old_line2 = __mxu2_shufv(zero, old_line2, cvtbyte_short);
							old_line3 = __mxu2_shufv(zero, old_line3, cvtbyte_short);

							new_line0 = __mxu2_shufv(zero, new_line0, cvtbyte_short);
							new_line1 = __mxu2_shufv(zero, new_line1, cvtbyte_short);
							new_line2 = __mxu2_shufv(zero, new_line2, cvtbyte_short);
							new_line3 = __mxu2_shufv(zero, new_line3, cvtbyte_short);

							old_line0 = __mxu2_sub_h(old_line0, new_line0);
							old_line1 = __mxu2_sub_h(old_line1, new_line1);
							old_line2 = __mxu2_sub_h(old_line2, new_line2);
							old_line3 = __mxu2_sub_h(old_line3, new_line3);
							/*************line[4,7]************/
							old_line4 = __mxu2_lu1q(old_ptr + sstep*4 + woffset, 0);
							old_line5 = __mxu2_lu1q(old_ptr + sstep*5 + woffset, 0);
							old_line6 = __mxu2_lu1q(old_ptr + sstep*6 + woffset, 0);
							old_line7 = __mxu2_lu1q(old_ptr + sstep*7 + woffset, 0);

							new_line4 = __mxu2_lu1q(new_ptr + sstep*4 + woffset, 0);
							new_line5 = __mxu2_lu1q(new_ptr + sstep*5 + woffset, 0);
							new_line6 = __mxu2_lu1q(new_ptr + sstep*6 + woffset, 0);
							new_line7 = __mxu2_lu1q(new_ptr + sstep*7 + woffset, 0);

							old_line4 = __mxu2_shufv(zero, old_line4, cvtbyte_short);
							old_line5 = __mxu2_shufv(zero, old_line5, cvtbyte_short);
							old_line6 = __mxu2_shufv(zero, old_line6, cvtbyte_short);
							old_line7 = __mxu2_shufv(zero, old_line7, cvtbyte_short);

							new_line4 = __mxu2_shufv(zero, new_line4, cvtbyte_short);
							new_line5 = __mxu2_shufv(zero, new_line5, cvtbyte_short);
							new_line6 = __mxu2_shufv(zero, new_line6, cvtbyte_short);
							new_line7 = __mxu2_shufv(zero, new_line7, cvtbyte_short);

							old_line4 = __mxu2_sub_h(old_line4, new_line4);
							old_line5 = __mxu2_sub_h(old_line5, new_line5);
							old_line6 = __mxu2_sub_h(old_line6, new_line6);
							old_line7 = __mxu2_sub_h(old_line7, new_line7);
							/********************SUM*******************/
							old_line0 = __mxu2_add_h(old_line0, old_line4);
							old_line1 = __mxu2_add_h(old_line1, old_line5);
							old_line2 = __mxu2_add_h(old_line2, old_line6);
							old_line3 = __mxu2_add_h(old_line3, old_line7);

							old_line0 = __mxu2_add_h(old_line0, old_line1);
							old_line2 = __mxu2_add_h(old_line2, old_line3);
							old_line0 = __mxu2_add_h(old_line0, old_line2);
							sum_blur = __mxu2_add_h(sum_blur, old_line0);
						}//cell_in_wblur
					}//cell_in_hblur
					__mxu2_su1q(sum_blur, sumarray, 0);
					data_diff = sumarray[0] + sumarray[1];
					data_diff += sumarray[2] + sumarray[3];
					data_diff += sumarray[4] + sumarray[5];
					data_diff += sumarray[6] + sumarray[7];
					data_diff = abs(data_diff);
					count = count + (data_diff > diff_thres);
					blur_diff_array[block_index] = data_diff > diff_thres_2;
					block_index++;
				}//blur_in_wblock
			}//blur_in_hblock
			/*************dst**************/
			data_result_tmp = count >= diff_num_thres;
			mv_block_num = mv_block_num + data_result_tmp;

			bool block_status = (mv_block_num <= mv_block_limit);
			block_index = 0;
			new_skip_height = i*blur_num_inhblock;
			for(m = 0; m < blur_num_inhblock; m++, new_skip_height++){
				new_skip_width = j*blur_num_inwblock;

				restore_in_h = skip_h_size;
				for(n = 0; n < blur_num_inwblock; n++, block_index ++, new_skip_width++){
					data_result_old = *(psn_addr + new_skip_height*skip_step + new_skip_width);
					data_result = data_result_tmp && ((blur_diff_array[block_index] && block_status) + !block_status);
					data_result = 1 - data_result;
					data_result = (~data_result+1)&(data_result_old + (data_result_old < psn_max_num));
					*(psn_addr + new_skip_height*skip_step + new_skip_width) = data_result;

					restore_in_w = (skip_w_size>>1);
					for( l = 0; l < restore_in_h; l++){
						for( k = 0; k < restore_in_w; k++){
							*(_dst + (new_block_height + m * skip_h_size + l) * dstep + ((new_block_width + n * skip_w_size)>>1) + k) = (data_result << 4) | data_result;
						}
					}
				}
			}
			/**************dst***************/
		}//block_in_w
	}//block_in_h

	//padding first 8 lines

	for( i = 0; i < 8; i++){
		memcpy(_dst + i*dstep, _dst + 8 * dstep, dstep);
	}

	//padding last residual lines
	if(block_h_res != 0){
		int h_last_tmp = height - block_h_res;
		for( i = h_last_tmp; i < height; i++){
			memcpy(_dst + i*dstep, _dst + (h_last_tmp - 1) * dstep, dstep);
		}
	}

	//padding first 8 columns
	for( i = 0; i < height; i++){
		for( j = 0; j < 4; j++){
			*(_dst + i * width / 2 + j) = *(_dst + i * width / 2 + 4);
		}
	}

	//padding last residual columns
	if(block_w_res != 0){
		int w_last_tmp = (width - block_w_res) / 2;
		for( i = 0; i < height; i++){
			for( j = w_last_tmp; j < width/2; j++){
				*(_dst + i * width / 2 + j) = *(_dst + i * width / 2 + w_last_tmp - 1);
			}
		}
	}
}

#if 0
int main()
{
	int width = 1920;
	int height = 1080;
	uint8_t *srcOld = (uint8_t *)malloc(height * width * sizeof(uint8_t));
	uint8_t *srcNew = (uint8_t *)malloc(height * width * sizeof(uint8_t));
	uint8_t *srcUV  = (uint8_t *)malloc(height/2 * width * sizeof(uint8_t));
	memset(srcOld, 0, width * height);
	memset(srcNew, 0, width * height);
	uint8_t *dst = (uint8_t *)malloc(height * width/2 * sizeof(uint8_t));
	memset(dst, 0, width/2 * height);
	uint8_t *dstc = (uint8_t *)malloc(height * width/2 * sizeof(uint8_t));
	memset(dstc, 0, width/2 * height);
	uint8_t* psn_addr1 = (uint8_t *)malloc(height * width * 2 *sizeof(uint8_t));
	uint8_t* psn_addr2 = psn_addr1 + height * width * sizeof(uint8_t);
	int si;
	for(si =0; si < height*width;si++)
		psn_addr1[si] = 0;
	memcpy(psn_addr2, psn_addr1, width * height);

	FILE *fp = fopen("aaa.yuv", "rb");
	if(0 == fp){
		printf("open file error\n");
		exit(0);
		return ;
	}
	/*******paramter******/
	int block_h_size = 64;
	int block_w_size = 64;
	int skip_h_size = 16;
	int skip_w_size = 16;
	int blur_radius_size = 8;
	int blur_diff_thres = 5;
	int diff_num_thres = 2;

	int mv_block_limit = 1000;
	int pixel_blur_diff_thres = 3;
	int block_h_size_ds = 32;
	int block_w_size_ds = 32;
	int skip_h_size_ds = 16;
	int skip_w_size_ds = 16;
	int blur_radius_size_ds = 8;
	int blur_diff_thres_change_ds = 2;
	int mv_block_limit_ds = 100;
	/*******process******/
	int psn_max_num = 10;
	fread(srcOld, 1, height * width, fp);
	fread(srcUV, 1, height/2 * width, fp);
	for(si = 0; si < 100; si++)
	{

		printf("Frame number is %d \n", si);

		fread(srcNew, 1, height * width, fp);
		fread(srcUV, 1, height/2 * width, fp);

		int timeout;
		struct timeval timeval, timeval1;
		gettimeofday(&timeval, NULL);
		filter2D_SIMD(dst, psn_addr1, srcOld, srcNew, width, height, block_h_size, block_w_size, skip_h_size, skip_w_size,
			      blur_radius_size, blur_diff_thres, pixel_blur_diff_thres, diff_num_thres, width, height, psn_max_num,
			      mv_block_limit, block_h_size_ds, block_w_size_ds, skip_h_size_ds, skip_w_size_ds, blur_radius_size_ds,
			      blur_diff_thres_change_ds, mv_block_limit_ds);
		gettimeofday(&timeval1, NULL);
		timeout = (timeval1.tv_sec - timeval.tv_sec) * 1000 + (timeval1.tv_usec - timeval.tv_usec)/1000;
		printf("\ntimeout : %d ms\n", timeout);

		printf("----------------------------------------------------------\n");
		gettimeofday(&timeval, NULL);
		filter2D(dstc, psn_addr2, srcOld, srcNew, width, height, block_h_size, block_w_size, skip_h_size, skip_w_size,
			 blur_radius_size, blur_diff_thres, pixel_blur_diff_thres, diff_num_thres, width, height, psn_max_num,
			 mv_block_limit, block_h_size_ds, block_w_size_ds, skip_h_size_ds, skip_w_size_ds, blur_radius_size_ds,
			 blur_diff_thres_change_ds, mv_block_limit_ds);
		gettimeofday(&timeval1, NULL);
		timeout = (timeval1.tv_sec - timeval.tv_sec) * 1000 + (timeval1.tv_usec - timeval.tv_usec)/1000;
		printf("\ntimeout : %d ms\n", timeout);

		int i, j;
		int dstep = width/2;
		for(i=0;i<height;i++){
			uint8_t* rdata = dstc+i*dstep;
			uint8_t* edata = dst+i*dstep;
			for(j = 0; j<width/2;j++){
				if(rdata[j] != edata[j])
				{
					printf("dst compare error [%d, %d]: right : %d, error : %d\n",i,j,rdata[j],edata[j]);
					return 0;
				}

				if((i == 0) && (j == 0)){
					printf("Position [0 0] value is %d \n", rdata[j] & 0xF);
				}

				if((i == 1079) && (j == 959)){
					printf("Position [1079 959] value is %d \n", rdata[j] & 0xF);
				}

				if((i == 880) && (j == 600)){
					printf("Position [880 600] value is %d \n", rdata[j] & 0xF);
				}

				if((i == 880) && (j == 608)){
					printf("Position [880 608] value is %d \n", rdata[j] & 0xF);
				}

				if((i == 896) && (j == 600)){
					printf("Position [880 608] value is %d \n", rdata[j] & 0xF);
				}

				if((i == 990) && (j == 550)){
					printf("Position [990 550] value is %d \n", rdata[j] & 0xF);
				}

				if((i == 750) && (j == 430)){
					printf("Position [750 430] value is %d \n", rdata[j] & 0xF);
				}

			}
		}
		printf("dst compare pass!!!!!\n");
		int sstep = width;
		for(i=0;i<height;i++){
			uint8_t* rdata = psn_addr1+i*sstep;
			uint8_t* edata = psn_addr2+i*sstep;
			for(j = 0; j<width;j++){
				if(rdata[j] != edata[j])
				{
					printf("psn compare error [%d, %d]: right : %d, error : %d\n",i,j,rdata[j],edata[j]);
					return 0;
				}
			}
		}
		printf("psn compare pass!!!!!\n");

		memcpy(srcOld, srcNew, height*width);
	}

	free(srcOld);
	free(srcNew);
	free(dst);
	free(dstc);
	free(psn_addr1);
	fclose(fp);
	return 0;
}

#endif
